﻿#ifndef MATRIX4D_H
#define MATRIX4D_H

#include <QString>
#include "memath.h"
#include "MePoint.h"
#include "matrix.h"

//#define Matrix4d Matrix<double>(4,4)
typedef Matrix<double> Matrixd;

class Matrix3d : public Matrixd
{
public:
    Matrix3d():Matrix< double >( 3, 3 ){}
    // 返回的时候用到
    Matrix3d(Matrixd const & copyMatrix) {
        rows= 3;
        columns = 3;
        allocate(3, 3);
        for ( unsigned row = 0; row < rows; ++row )
            for ( unsigned column = 0; column < columns; ++column )
                this->matrix[ row ][ column ] = copyMatrix.get(row, column);
    }

    /**
     * @brief 对点进行运算
     * @param v 输入点
     * @param out 输出点
     */
    void multiplyPoint3D(double v[3], double out[3]);
};

class Matrix4d : public Matrixd
{
public:
    Matrix4d():Matrix< double >( 4, 4 ){}

    // 返回的时候用到
    Matrix4d(Matrixd const & copyMatrix) {
        rows= 4;
        columns = 4;
        allocate(4, 4);
        for ( unsigned row = 0; row < 4; ++row )
            for ( unsigned column = 0; column < 4; ++column )
                this->matrix[ row ][ column ] = copyMatrix.get(row, column);
    }

    std::vector<double> &getRow(int row) { return this->matrix[row]; }

    double &operator()(unsigned row, unsigned column) {
        return matrix[ row ][ column ];
    }

//    std::vector<double> &operator[](unsigned row) {
//        return matrix[ row ];
//    }

    /**
     * Set all of the elements to zero.
     */
    static void zero(double elements[16]);
    /**
     * Set equal to Identity matrix
     */
    void identity();
    static void identity(double elements[16]);

    /**
     * @brief 设置偏移
     * @param t
     */
    void translate(double t[3]);
    /**
     * @brief 对点进行运算，要求矩阵最后一行为 [0, 0, 0, 1]
     * @param v 输入点
     * @param out 输出点
     */
    void multiplyPoint3D(double v[3], double out[3]);
    void multiplyPoint3D(MePoint3D &p, MePoint3D &outP);
    MePoint3D multiplyPoint3D(MePoint3D &p);
    void multiplyPoint4D(double v[4], double out[4]);

    // TODO: getInverse 可能有问题
    /// 获取齐次矩阵的逆, get
    Matrix4d getInverseHomogeneous() const;

    QString toQString() const;

    static Matrix4d fromString(const QString &matrixString);
    static Matrix4d fromString(const std::string &matrixString);

};

class MatrixUtil
{
public:
    /** 将很小的数转成zero */
    static void convertTinyToZero(Matrixd &m);
    static void convertTinyToZero(Matrixd *m);

    /**
     * @brief 比较两个矩阵是否在一定的误差内相等
     * @param mA 输入A
     * @param mB 输入B
     * @param eps 误差范围
     * @return true 相等， false 不等
     */
    static bool isMatrixdEqual(const Matrixd &mA, const Matrixd &mB, const double &eps=ME_TINY);
};

#endif // MATRIX4D_H
